/**
 * @file ebex_genetic_optimize.c
 *
 * @date Oct 21, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */


#include <stdbool.h>

#include <ebex_sort.h>
#include <ebex_genetic.h>
#include <ebex_genetic_mutate.h>
#include <ebex_genetic_seed.h>

#include <ebex_genetic_optimize.h>


void ebex_genetic_mating(population_t *m_pop)
{
	organism_t *mother, *father; /* Parent entities. */
	organism_t *son, *daughter; /* Child entities. */

	elog(log_level_extreme, "*** Mating cycle ***");

	if (m_pop->crossover_ratio <= 0.0)
		return;

	m_pop->select_state = 0;

	/* Select pairs of entities to mate via crossover. */
#	pragma intel omp parallel taskq
	while (!ebex_genetic_select_two(m_pop, &mother, &father, m_pop->select_two_type))
	{

		if (mother && father)
		{
#			pragma intel omp task \
				private(son,daughter) captureprivate(mother,father)
			{
				elog(log_level_extreme,
					"Mating between %d (rank %d fitness %f) and %d (rank %d fitness %f)",
					genetic_get_index(m_pop, mother), genetic_organism_get_rank(m_pop, mother), mother->fitness,
					genetic_get_index(m_pop, father), genetic_organism_get_rank(m_pop, father), father->fitness);

				son = genetic_get_new_organism(m_pop);
				daughter = genetic_get_new_organism(m_pop);
				ebex_genetic_mate(m_pop, father, mother, son, daughter);
			}
		}
		else
		{
			elog( log_level_verbose, "Crossover not performed.");
		}
	}

	return;
}

void ebex_genetic_mutation(population_t *m_pop)
{
	organism_t *parent; /* Parent entities. */
	organism_t *child; /* Child entities. */

	elog(log_level_extreme, "*** Mutation cycle ***");

	if (m_pop->mutation_ratio <= 0.0)
		return;

	m_pop->select_state = 0;

#	pragma intel omp parallel taskq
	/// Select organisms to mutate (asexual reproduction).  Parents are kept at this stage.
	while (!ebex_genetic_select_one(m_pop, &parent, m_pop->select_one_type))
	{

		if (parent)
		{
#		pragma intel omp task private(child) captureprivate(parent)
			{
				elog(log_level_extreme,
					"Mutation of %d (rank %d fitness %f)", genetic_get_index(m_pop, parent), genetic_organism_get_rank(m_pop, parent), parent->fitness);

				child = genetic_organism_clone(m_pop, parent);
				ebex_genetic_mutate(m_pop, child, m_pop->mutation_type);
				m_pop->verify(m_pop, child);
			}
		}
		else
		{
			elog( log_level_verbose, "Mutation not performed.");
		}
	}

	return;
}

void ebex_genetic_eval_pop(population_t *m_pop)
{
	size_t i;

#	pragma omp parallel for \
		shared(m_pop) private(i) schedule(static)
	for (i = 0; i < m_pop->size; i++)
	{
		if (m_pop->evaluate(m_pop, m_pop->sorted_organisms[i]) == false)
			m_pop->sorted_organisms[i]->fitness = GENETIC_MIN_FITNESS;
	}

	return;
}

void ebex_genetic_adapt_eval(population_t *m_pop)
{
	size_t i;
	organism_t *adult = NULL;
	int adultrank;

	/// Darwinian evolution -- No Adaptation
	if (m_pop->scheme == EVOLUTION_MODE_DARWIN)
	{
		elog(log_level_extreme, "*** Fitness Evaluations ***");

#		pragma omp parallel for \
			shared(m_pop) private(i) schedule(static)
		for (i = m_pop->orig_size; i < m_pop->size; i++)
		{
			if (m_pop->evaluate(m_pop, m_pop->sorted_organisms[i]) == false)
				m_pop->sorted_organisms[i]->fitness = GENETIC_MIN_FITNESS;
		}

		return;
	}
	else
	{ /// Some kind of adaptation is required.  First reevaluate parents, as needed, then children.

		elog(log_level_extreme, "*** Adaptation and Fitness Evaluations ***");

		if ((m_pop->scheme & EVOLUTION_MODE_BALDWIN_PARENTS) != 0)
		{
#			pragma omp parallel for \
				shared(m_pop) private(i,adult) schedule(static)
			for (i = 0; i < m_pop->orig_size; i++)
			{
				adult = m_pop->adapt(m_pop, m_pop->sorted_organisms[i]);
				m_pop->sorted_organisms[i]->fitness = adult->fitness;
				genetic_free_organism(m_pop, adult);
			}
		}
		else if ((m_pop->scheme & EVOLUTION_MODE_LAMARCK_PARENTS) != 0)
		{
#			pragma omp parallel for \
				shared(m_pop) private(i,adult,adultrank) schedule(static)
			for (i = 0; i < m_pop->orig_size; i++)
			{
				adult = m_pop->adapt(m_pop, m_pop->sorted_organisms[i]);
				adultrank = genetic_organism_get_rank(m_pop, adult);
				EBEX_SWAP(m_pop->sorted_organisms[i], m_pop->sorted_organisms[adultrank]);
				genetic_free_rank(m_pop, adultrank);
			}
		}

		if ((m_pop->scheme & EVOLUTION_MODE_BALDWIN_CHILDREN) != 0)
		{
#			pragma omp parallel for \
				shared(m_pop) private(i,adult) schedule(static)
			for (i = m_pop->orig_size; i < m_pop->size; i++)
			{
				adult = m_pop->adapt(m_pop, m_pop->sorted_organisms[i]);
				m_pop->sorted_organisms[i]->fitness = adult->fitness;
				genetic_free_organism(m_pop, adult);
			}
		}
		else if ((m_pop->scheme & EVOLUTION_MODE_LAMARCK_CHILDREN) != 0)
		{
#			pragma omp parallel for \
				shared(m_pop) private(i,adult,adultrank) schedule(static)
			for (i = m_pop->orig_size; i < m_pop->size; i++)
			{
				adult = m_pop->adapt(m_pop, m_pop->sorted_organisms[i]);
				adultrank = genetic_organism_get_rank(m_pop, adult);
				EBEX_SWAP(m_pop->sorted_organisms[i], m_pop->sorted_organisms[adultrank]);
				genetic_free_rank(m_pop, adultrank);
			}
		}
	}

	return;
}

void ebex_genetic_elitism(population_t *m_pop)
{

	elog(log_level_extreme, "*** Survival of the fittest ***");

	switch(m_pop->elitism)
	{
		case GENETIC_SELECTION_PARENTS_SURVIVE:
			break;
		case GENETIC_SELECTION_PARENTS_DIE:
			while (m_pop->orig_size)
			{
				m_pop->orig_size--;
				genetic_free_rank(m_pop, m_pop->orig_size);
			}
			break;
		case GENETIC_SELECTION_ONE_PARENT_SURVIVES:

			while (m_pop->orig_size > 1)
			{
				m_pop->orig_size--;
				genetic_free_rank(m_pop, m_pop->orig_size);
			}

			break;
		case GENETIC_SELECTION_RESCORE_PARENTS:
			elog(log_level_extreme, "*** Fitness Re-evaluations ***");

#			pragma omp parallel for \
			   shared(m_pop) schedule(static)
			for (size_t i = m_pop->orig_size; i < m_pop->size; i++)
			{
				if (m_pop->evaluate(m_pop, m_pop->sorted_organisms[i]) == false)
					m_pop->sorted_organisms[i]->fitness = GENETIC_MIN_FITNESS;
			}

			break;
		default:
			die("Unknown elitism mode");
			break;
	}

	sort_population(m_pop);
	genetic_genocide(m_pop, m_pop->stable_size);

	return;
}

